clear all
close all
clc
%s = fopen('20 hz 16 samples 0 shift cold startup.txt');
%s = fopen('20 hz 32 samples 0 shift 2.txt');
%s = fopen('20 hz 16 samples 0 shift tenu main.txt');
%s = fopen('real time 16 samples 0 shift.txt');
%s = fopen('real time 16 samples 0 shift cold startup.txt');
%s = fopen('real time 16 samples 0 shift position change 2.txt');
s = fopen('real time 16 samples 0 shift with motor2.txt');


out2 = [];
while ~feof(s)
    out = fscanf(s,'%d %d %d %d %d %d', 6);
    if (numel(out)>5)
        if (abs(out(1)) < 100 && abs(out(2)) < 100 && abs(out(3)) < 100 && abs(out(5)) < 10000)
        out2 = [out2 out'];
        end
    end
end

fclose(s);


figure;
    p = out2(1:6:end)';
    plot(p - mean(p)); hold on;
    q = out2(2:6:end)';
    plot(q - mean(q),'r');hold on;
    r = out2(3:6:end)';
    plot(r - mean(r),'g');
    title('Gyros'); 
    legend('X', 'Y', 'Z');
    
    figure;
    xacc = out2(4:6:end)';
    plot(xacc - mean(xacc)); hold on;
    yacc = out2(5:6:end)';
    plot(yacc - mean(yacc),'r');hold on;
    zacc = out2(6:6:end)';
    plot(zacc - mean(zacc),'g');
    title('Acceleros'); 
    legend('X', 'Y', 'Z');
    
    
    p1 = bitshift(p, -1);
    q1 = bitshift(q, -1);
    r1 = bitshift(r, -1);
    xacc1 = bitshift(xacc, -1);
    yacc1 = bitshift(yacc, -1);
    zacc1 = bitshift(zacc, -1);
    
    p2 = bitshift(p, -2);
    q2 = bitshift(q, -2);
    r2 = bitshift(r, -2);
    xacc2 = bitshift(xacc, -2);
    yacc2 = bitshift(yacc, -2);
    zacc2 = bitshift(zacc, -2);
    
    p3 = bitshift(p, -3);
    q3 = bitshift(q, -3);
    r3 = bitshift(r, -3);
    xacc3 = bitshift(xacc, -3);
    yacc3 = bitshift(yacc, -3);
    zacc3 = bitshift(zacc, -3);
    
    p4 = bitshift(p, -4);
    q4 = bitshift(q, -4);
    r4 = bitshift(r, -4);
    xacc4 = bitshift(xacc, -4);
    yacc4 = bitshift(yacc, -4);
    zacc4 = bitshift(zacc, -4);

    
%     p1 = p/2;
%     q1 = q/2;
%     r1 = r/2;
%     xacc1 = xacc/2;
%     yacc1 = yacc/2;
%     zacc1 = zacc/2;
%     
%     p2 = p/4;
%     q2 = q/4;
%     r2 = r/4;
%     xacc2 = xacc/4;
%     yacc2 = yacc/4;
%     zacc2 = zacc/4;
%     
%     p3 = p/8;
%     q3 = q/8;
%     r3 = r/8;
%     xacc3 = xacc/8;
%     yacc3 = yacc/8;
%     zacc3 = zacc/8;
%     
%     p4 = p/16;
%     q4 = q/16;
%     r4 = r/16;
%     xacc4 = xacc/16;
%     yacc4 = yacc/16;
%     zacc4 = zacc/16;

    
    % 16 samples, 0 shift
    %([mean(p) mean(q) mean(r) ; mean(xacc) mean(yacc)  mean(zacc)] / 16);
    %(10*[var(p) var(q) var(r) ;  var(xacc) var(yacc)  var(zacc)] / 256);
    
    % 16 samples, 1 shift
    %([mean(p1) mean(q1) mean(r1) ; mean(xacc1) mean(yacc1)  mean(zacc1)] / 8);
    %(10*[var(p1) var(q1) var(r1) ;  var(xacc1) var(yacc1)  var(zacc1)] / 64);
    
    % 16 samples, 2 shift
    %([mean(p2) mean(q2) mean(r2) ; mean(xacc2) mean(yacc2)  mean(zacc2)] / 4);
    %(10*[var(p2) var(q2) var(r2) ;  var(xacc2) var(yacc2)  var(zacc2)] / 16);
    
    % 16 samples, 3 shift
    %([mean(p3) mean(q3) mean(r3) ; mean(xacc3) mean(yacc3)  mean(zacc3)] / 2);
    %(10*[var(p3) var(q3) var(r3) ;  var(xacc3) var(yacc3)  var(zacc3)] / 4);
    
    % 16 samples, 4 shift
    %[mean(p4) mean(q4) mean(r4) ; mean(xacc4) mean(yacc4)  mean(zacc4)];
    %10*[var(p4) var(q4) var(r4) ;  var(xacc4) var(yacc4)  var(zacc4)];
%     
%     figure;
%     plot([0 1 2 3 4],[mean(p)/16 mean(p1)/8 mean(p2)/4 mean(p3)/2 mean(p4)]); hold on
%     plot([0 1 2 3 4],[mean(q)/16 mean(q1)/8 mean(q2)/4 mean(q3)/2 mean(q4)],'r'); hold on
%     plot([0 1 2 3 4],[mean(r)/16 mean(r1)/8 mean(r2)/4 mean(r3)/2 mean(r4)],'g');
%     legend('X', 'Y', 'Z');
%     xlabel('Number of right shifting');
%     title('Gyros means for several shifting (normalized to 10bits)'); 
%     
%     figure;
%     plot([0 1 2 3 4],[mean(xacc)/16 mean(xacc1)/8 mean(xacc2)/4 mean(xacc3)/2 mean(xacc4)]); hold on
%     plot([0 1 2 3 4],[mean(yacc)/16 mean(yacc1)/8 mean(yacc2)/4 mean(yacc3)/2 mean(yacc4)],'r'); hold on
%     plot([0 1 2 3 4],[mean(zacc)/16 mean(zacc1)/8 mean(zacc2)/4 mean(zacc3)/2 mean(zacc4)],'g');
%     legend('X', 'Y', 'Z');
%     xlabel('Number of right shifting');
%     title('Acceleros means for several shifting (normalized to 10bits)'); 
%     
    figure;
    plot([0 1 2 3 4],[var(p)/256 var(p1)/64 var(p2)/16 var(p3)/4 var(p4)]); hold on
    plot([0 1 2 3 4],[var(q)/256 var(q1)/64 var(q2)/16 var(q3)/4 var(q4)],'r'); hold on
    plot([0 1 2 3 4],[var(r)/256 var(r1)/64 var(r2)/16 var(r3)/4 var(r4)],'g');
    legend('X', 'Y', 'Z');
    xlabel('Number of right shifting');
    title('Gyros variances for several shifting (normalized to 10bits)'); 
    
    figure;
    plot([0 1 2 3 4],[var(xacc)/256 var(xacc1)/64 var(xacc2)/16 var(xacc3)/4 var(xacc4)]); hold on
    plot([0 1 2 3 4],[var(yacc)/256 var(yacc1)/64 var(yacc2)/16 var(yacc3)/4 var(yacc4)],'r'); hold on
    plot([0 1 2 3 4],[var(zacc)/256 var(zacc1)/64 var(zacc2)/16 var(zacc3)/4 var(zacc4)],'g');
    legend('X', 'Y', 'Z');
    xlabel('Number of right shifting');
    title('Acceleros variances for several shifting (normalized to 10bits)'); 
    
    
    
%     figure;
%     plot(p2 - mean(p2)); hold on;
%     plot(q2 - mean(q2),'r');hold on;
%     plot(r2 - mean(r2),'g');
%     title('Gyros with 2 right shifting'); 
%     legend('X', 'Y', 'Z');
%     
%     figure;
%     plot(xacc2 - mean(xacc2)); hold on;
%     plot(yacc2 - mean(yacc2),'r');hold on;
%     plot(zacc2 - mean(zacc2),'g');
%     title('Acceleros with 2 right shifting'); 
%     legend('X', 'Y', 'Z');
%     
    figure;
    plot(p4 - mean(p4)); hold on;
    plot(q4 - mean(q4),'r');hold on;
    plot(r4 - mean(r4),'g');
    title('Gyros with 4 right shifting'); 
    legend('X', 'Y', 'Z');
    
    figure;
    plot(xacc4 - mean(xacc4)); hold on;
    plot(yacc4 - mean(yacc4),'r');hold on;
    plot(zacc4 - mean(zacc4),'g');
    title('Acceleros with 4 right shifting'); 
    legend('X', 'Y', 'Z');



  debut = 750;
    fin = 1000;
    
    [var(p(debut:fin))
    var(q(debut:fin))
    var(r(debut:fin))
    var(xacc(debut:fin))
    var(yacc(debut:fin))
    var(zacc(debut:fin))]

   [var(p4(debut:fin))
    var(q4(debut:fin))
    var(r4(debut:fin))
    var(xacc4(debut:fin))
    var(yacc4(debut:fin))
    var(zacc4(debut:fin))]*256

    